package com.crunch.physics;

import com.crunch.math.MathOps;
import com.crunch.math.Ray2f;
import com.crunch.math.Vector2f;

/**
 * Math functions related to physics - package private.
 */
class PhysicsMath {
	public static class SweepResult {
		public float t;         // time of intersection, or infinity if no intersection occurred
		public Vector2f point;  // point of intersection
		public Vector2f normal; // normal vector of intersection
	}

	// returns the result of sweeping the given capsule against the given segment using the given origin/direction
	public static void sweepCapsuleAgainstSegment(Capsule capsule, Vector2f upVector, Vector2f origin, Vector2f direction,
	                                              Vector2f[] segmentPoints, Vector2f segmentNormal,
	                                              SweepResult result) {
		if (capsule.getHeight() == 0.0f) {
			sweepCircleAgainstSegment(capsule.getRadius(), origin, direction, segmentPoints, segmentNormal, result);
			return;
		}

		// initialize result
		result.t = Float.POSITIVE_INFINITY;

		if (direction.isZero()) {
			// no intersection
			return;
		}

		// determine capsule "endpoints"
		Vector2f centerOffset = upVector.multiply(capsule.getHeight()*0.5f);
		Vector2f capsulePoints[] = {
				origin.subtract(centerOffset),
				origin.add(centerOffset)
		};

		float c = MathOps.dot(segmentPoints[0], segmentNormal);

		// first we test intersection with the "line" of the segment
		// if we're moving away from the segment, we can't intersect with it
		float nDotD = MathOps.dot(segmentNormal, direction);
		if (nDotD < 0.0f) {
			// find the closest capsule point to the line
			float nDotP0 = MathOps.dot(segmentNormal, capsulePoints[0]);
			float nDotP1 = MathOps.dot(segmentNormal, capsulePoints[1]);
			int closestPoint = (nDotP0 < nDotP1) ? 0 : 1;
			// if behind plane by radius, discard result
			float closestDist = MathOps.dot(segmentNormal, capsulePoints[closestPoint]) - c;
			if (closestDist >= 0.0f) {
				// parameterize point by sweep vector: p(t) = p0 + t*v
				// distance is n.(p0 + t*v) - c
				// solve for when distance is r:
				// r = n.(p0 + t*v) - c
				// r + c = n.p0 + t*n.v
				// t = (r + c - n.p0) / n.v
				// if n.v = 0, there is no intersection since we are moving parallel; we have already checked this
				float num = capsule.getRadius() - closestDist;
				float t = num / nDotD;
				Vector2f linePoint = direction.multiply(t).addAssign(capsulePoints[closestPoint]).subtractAssign(segmentNormal.multiply(capsule.getRadius()));
				if (pointInSegment(linePoint, segmentPoints)) {
					// intersection occurred on line
					result.t = Math.max(0.0f, t); // clamp to 0
					result.point.set(linePoint);
					result.normal.set(segmentNormal);
					return;
				}
			}
		} else {
			// we're not moving toward the plane
			return;
		}

		// now test each vertex
		Vector2f capsuleVec = capsulePoints[1].subtract(capsulePoints[0]);
		Vector2f t = new Vector2f(); // hold 2 t-values
		for (int i = 0; i < 2; ++i) {
			// sweep against the point - returns false if we're moving away
			if (sweepLineRadiusAgainstPoint(capsulePoints[0], upVector.rotateCcw(), capsule.getRadius(), direction,
					segmentPoints[i], t)) {
				float t0 = t.get(0);
				float t1 = t.get(1);

				// if both negative, intersection occurred behind us
				// if t0 negative and t1 positive, we're "embedded" and either trying to move further in our out
				// however, we've already handled the "move out" case, so we're trying to move further it, in which case t = 0
				// if t0 and t1 are both positive, take the smallest value for the first intersection
				if (t1 > 0.0f) {
					float clampedT0 = Math.max(t0, 0.0f);
					// make sure the point of intersection is actually on the line segment of the capsule
					// segmentPoints[i] - direction*t - capsulePoints[0]
					Vector2f projPoint =
							direction.multiply(-clampedT0).subtractAssign(capsulePoints[0]).addAssign(segmentPoints[i]);
					float proj = MathOps.dot(capsuleVec, projPoint);
					if (proj >= 0.0f && proj <= capsuleVec.magnitudeSquared()) {
						if (Float.isInfinite(result.t) || clampedT0 < result.t) {
							result.t = clampedT0;
							result.point.set(segmentPoints[i]);
							result.normal.set(projPoint);
							// remove component that is not in the direction of the normal
							Vector2f nonNormalComp = capsuleVec.multiply(
									MathOps.dot(projPoint, capsuleVec) / capsuleVec.magnitudeSquared());
							result.normal.subtractAssign(nonNormalComp);
							result.normal.negateAssign();
							if (t0 < 0.0f) {
								// embedded
								result.normal.normalizeAssign();
							} else {
								result.normal.divideAssign(capsule.getRadius());
							}
							if (t0 <= 0.0f) {
								// can't do better than 0, so return now
								return;
							}
						}
					}
				}
			}

			// now check each endpoint of the capsule separately
			for (int e = 0; e < 2; ++e) {
				// sweep against the point - returns false if we're moving away
				if (!sweepPointRadiusAgainstPoint(capsulePoints[e], capsule.getRadius(), direction, segmentPoints[i], t)) {
					continue;
				}
				float t0 = t.get(0);
				float t1 = t.get(1);

				// if both negative, intersection occurred behind us
				// if t0 negative and t1 positive, we're "embedded" and either trying to move further in our out
				// however, we've already handled the "move out" case, so we're trying to move further it, in which case t = 0
				// if t0 and t1 are both positive, take the smallest value for the first intersection
				if (t1 > 0.0f) {
					float clampedT0 = Math.max(t0, 0.0f);
					if (Float.isInfinite(result.t) || clampedT0 < result.t) {
						result.t = clampedT0;
						result.point.set(segmentPoints[i]);
						if (t0 < 0.0f) {
							// embedded
							result.normal.set(capsulePoints[e]);
							result.normal.subtractAssign(segmentPoints[i]);
							result.normal.normalizeAssign();
						} else {
							result.normal.set(capsulePoints[e]);
							result.normal.addAssign(direction.multiply(t0)).subtractAssign(segmentPoints[i]);
							result.normal.divideAssign(capsule.getRadius());
						}
						if (t0 <= 0.0f) {
							// can't do better than 0, so return now
							return;
						}
					}
				}
			}
		}
	}

	// returns the result of sweeping the given circle against the given segment using the given origin/direction
	public static void sweepCircleAgainstSegment(float radius, Vector2f origin, Vector2f direction,
	                                             Vector2f[] segmentPoints, Vector2f segmentNormal,
	                                             SweepResult result) {
		// initialize result
		result.t = Float.POSITIVE_INFINITY;

		if (direction.isZero()) {
			// no intersection
			return;
		}

		float c = MathOps.dot(segmentPoints[0], segmentNormal);

		// first we test intersection with the "line" of the segment
		// if we're moving away from the segment, we can't intersect with it
		float nDotD = MathOps.dot(segmentNormal, direction);
		if (nDotD < 0.0f) {
			// if behind plane by radius, discard result
			float dist = MathOps.dot(segmentNormal, origin) - c;
			if (dist >= 0.0f) {
				// parameterize point by sweep vector: p(t) = p0 + t*v
				// distance is n.(p0 + t*v) - c
				// solve for when distance is r:
				// r = n.(p0 + t*v) - c
				// r + c = n.p0 + t*n.v
				// t = (r + c - n.p0) / n.v
				// if n.v = 0, there is no intersection since we are moving parallel; we have already checked this
				float num = radius - dist;
				float t = num / nDotD;
				Vector2f linePoint = origin.add(direction.multiply(t)).subtract(segmentNormal.multiply(radius));
				if (pointInSegment(linePoint, segmentPoints)) {
					// intersection occurred on line
					result.t = Math.max(0.0f, t); // clamp to 0
					result.point.set(linePoint);
					result.normal.set(segmentNormal);
					return;
				}
			}
		} else {
			// we're not moving toward the plane
			return;
		}

		// now test each vertex
		Vector2f t = new Vector2f(); // hold 2 t-values
		for (int i = 0; i < 2; ++i) {
			// sweep against the point - returns false if we're moving away
			if (!sweepPointRadiusAgainstPoint(origin, radius, direction, segmentPoints[i], t)) {
				continue;
			}
			float t0 = t.get(0);
			float t1 = t.get(1);

			// if both negative, intersection occurred behind us
			// if t0 negative and t1 positive, we're "embedded" and either trying to move further in our out
			// however, we've already handled the "move out" case, so we're trying to move further it, in which case t = 0
			// if t0 and t1 are both positive, take the smallest value for the first intersection
			if (t1 > 0.0f) {
				float clampedT0 = Math.max(t0, 0.0f);
				if (Float.isInfinite(result.t) || clampedT0 < result.t) {
					result.t = clampedT0;
					result.point.set(segmentPoints[i]);
					if (t0 < 0.0f) {
						// embedded
						result.normal.set(origin);
						result.normal.subtractAssign(segmentPoints[i]);
						result.normal.normalizeAssign();
					} else {
						result.normal.set(origin);
						result.normal.addAssign(direction.multiply(t0)).subtractAssign(segmentPoints[i]);
						result.normal.divideAssign(radius);
					}
					if (t0 <= 0.0f) {
						// can't do better than 0, so return now
						return;
					}
				}
			}
		}
	}

	// sweeps the sweep point against the reference point and stores the result in t
	// false is returned if no intersection occurs or if the sweep direction points away from the reference point
	public static boolean sweepPointRadiusAgainstPoint(Vector2f sweepPoint, float sweepRadius, Vector2f sweepDir,
	                                                   Vector2f refPoint,
	                                                    Vector2f t) {
		// bring to origin
		Vector2f newSweepPoint = sweepPoint.subtract(refPoint);
		float pDotD = MathOps.dot(newSweepPoint, sweepDir);
		// if we're sweeping away, return false
		if (pDotD >= 0.0f) {
			return false;
		}

		// sweep point
		float rad2 = sweepRadius*sweepRadius;

		// ray equation is p = o + td
		// want to solve for t such that p.p = r^2
		// r^2 = o.o + 2to.d + t^2d.d
		// t^2(d.d) + t(2o.d) + (o.o - r^2) = 0
		// solve using quadratic formula
		float a = sweepDir.magnitudeSquared();
		float b = 2.0f * pDotD;
		float c = newSweepPoint.magnitudeSquared() - rad2;
		float disc = b*b - 4.0f*a*c;
		if (disc < 0.0f) {
			return false;
		}
		disc = (float) Math.sqrt(disc);
		float invDenom = 0.5f / a;
		t.set((-b - disc) * invDenom, (-b + disc) * invDenom);
		if (t.get(0) > t.get(1)) {
			t.set(t.get(1), t.get(0));
		}
		return true;
	}

	// sweeps the sweep line against the reference point and stores the result in t
	// false is returned if no intersection occurs or if the sweep direction points away from the reference point
	public static boolean sweepLineRadiusAgainstPoint(Vector2f sweepLineOrigin, Vector2f sweepLineNormal, float sweepRadius, Vector2f sweepDir,
	                                                  Vector2f refPoint,
	                                                  Vector2f t) {
		// bring to origin
		Vector2f newSweepLineOrigin = sweepLineOrigin.subtract(refPoint);

		float nDotD = MathOps.dot(sweepLineNormal, sweepDir);
		float nDotP = MathOps.dot(sweepLineNormal, newSweepLineOrigin);
		// if we're sweeping away, return false
		if (nDotD == 0.0f || Math.signum(nDotD) == Math.signum(nDotP)) {
			return false;
		}

		// origin distance to line is:
		// d = -n.o
		// o is parameterized as o(t) = o0 + t*v
		// solve: +-r = -(n.(o + t*v))
		// +-r = -(n.o + t*n.v)
		// t = -(n.o +- r) / n.v
		float nDotO = MathOps.dot(sweepLineNormal, newSweepLineOrigin);
		float negInvNDotD = -1.0f / nDotD;
		float t0 = (nDotO - sweepRadius) * negInvNDotD;
		float t1 = (nDotO + sweepRadius) * negInvNDotD;
		if (t0 < t1) {
			t.set(t0, t1);
		} else {
			t.set(t1, t0);
		}
		return true;
	}

	// returns whether point p projects onto the given segment
	public static boolean pointInSegment(Vector2f p, Vector2f[] segmentPoints) {
		// project p
		Vector2f ab = segmentPoints[1].subtract(segmentPoints[0]);
		Vector2f ap = p.subtract(segmentPoints[0]);
		// compute projection (not normalized)
		float proj = MathOps.dot(ab, ap);
		return proj >= 0.0f && proj <= ab.magnitudeSquared();
	}

	public static class DistanceResult {
		public float distance;      // distance between objects, or POSITIVE_INFINITY
		public Vector2f pointA;     // closest point on object A to object B
		public Vector2f vectorAB;   // vector pointing from pointA to object B
	}

	// todo describe
	//public static boolean isCapsuleSegmentDistanceWithinRange(Capsule capsule, Vector2f upVector, Vector2f position,
	//                                                          Vector2f[] segmentPoints, Vector2f segmentNormal,
	//                                                          float maxDist);

	// if the closest point between the capsule and segment lies on the segment's plane, this distance is returned
	// otherwise, an infinite distance is returned
	public static void distanceCapsuleSegmentLine(Capsule capsule, Vector2f upVector, Vector2f position,
	                                              Vector2f[] segmentPoints, Vector2f segmentNormal,
	                                              DistanceResult result) {
		// initialize result
		result.distance = Float.POSITIVE_INFINITY;

		// determine capsule "endpoints"
		Vector2f centerOffset = upVector.multiply(capsule.getHeight()*0.5f);
		Vector2f capsulePoints[] = {
				position.subtract(centerOffset),
				position.add(centerOffset)
		};

		final float PARALLEL_THRESHOLD = 0.9999f;
		Vector2f lineVector = segmentNormal.rotateCcw();
		float dot = MathOps.dot(upVector, lineVector);
		float det = lineVector.x()*upVector.y() - upVector.x()*lineVector.y();
		if (capsule.getHeight() != 0.0f && (Math.abs(dot) >= PARALLEL_THRESHOLD || det == 0.0f)) {
			// lines are considered parallel
			// project capsule's points onto line
			Vector2f segmentVec = segmentPoints[1].subtract(segmentPoints[0]);
			float proj0 = MathOps.dot(capsulePoints[0].subtract(segmentPoints[0]), segmentVec);
			float proj1 = MathOps.dot(capsulePoints[1].subtract(segmentPoints[0]), segmentVec);
			float mag2 = segmentVec.magnitudeSquared();
			// if both points are out of range, reject
			if ((proj0 < 0.0f && proj1 < 0.0f) || (proj0 > mag2 && proj1 > mag2)) {
				return;
			}

			proj0 /= mag2;
			proj1 /= mag2;
			float mid = MathOps.clamp((proj0 + proj1) * 0.5f, 0.0f, 1.0f);
			// project back onto the capsule
			Vector2f segmentPoint = segmentVec.multiply(mid).addAssign(segmentPoints[0]);
			float capsuleProj = MathOps.dot(segmentPoint.subtract(position), upVector);
			Vector2f capsulePoint = upVector.multiply(capsuleProj).addAssign(position);

			Vector2f relCapsulePoint = capsulePoint.subtract(segmentPoints[0]);
			float dist = MathOps.dot(relCapsulePoint, segmentNormal);
			// discard result if embedded in plane past radius
			if (dist < 0.0f) {
				return;
			}

			result.distance = dist - capsule.getRadius();
			result.pointA.set(segmentNormal);
			result.pointA.multiplyAssign(-capsule.getRadius());
			result.pointA.addAssign(capsulePoint);
			result.vectorAB.set(segmentNormal);
			result.vectorAB.negateAssign();
		} else {
			float c = MathOps.dot(segmentPoints[0], segmentNormal);

			// find the closest capsule point to the line
			float nDotP0 = MathOps.dot(segmentNormal, capsulePoints[0]);
			float nDotP1 = MathOps.dot(segmentNormal, capsulePoints[1]);
			int closestPoint = (nDotP0 < nDotP1) ? 0 : 1;
			// if behind plane by radius, discard result
			float closestDist = MathOps.dot(segmentNormal, capsulePoints[closestPoint]) - c;
			if (closestDist >= 0.0f) {
				Vector2f linePoint = segmentNormal.multiply(-closestDist).addAssign(capsulePoints[closestPoint]);
				if (pointInSegment(linePoint, segmentPoints)) {
					result.distance = closestDist - capsule.getRadius();
					result.pointA.set(segmentNormal);
					result.pointA.multiplyAssign(-capsule.getRadius());
					result.pointA.addAssign(capsulePoints[closestPoint]);
					result.vectorAB.set(segmentNormal);
					result.vectorAB.negateAssign();
				}
			}
		}
	}

	// returns the distance between the capsule and the vertex
	public static void distanceCapsuleSegmentVertex(Capsule capsule, Vector2f upVector, Vector2f position,
	                                                Vector2f vertex,
	                                                DistanceResult result) {
		Vector2f closestPoint;
		if (capsule.getHeight() == 0.0f) {
			closestPoint = position;
		} else {
			// project vertex onto capsule's line
			float proj = MathOps.dot(vertex.subtract(position), upVector);
			float halfHeight = capsule.getHeight() * 0.5f;
			proj = MathOps.clamp(proj, -halfHeight, halfHeight);
			closestPoint = upVector.multiply(proj).addAssign(position);
		}

		result.vectorAB.set(vertex);
		result.vectorAB.subtractAssign(closestPoint);
		result.distance = result.vectorAB.magnitude();
		if (result.distance != 0.0f) {
			result.vectorAB.divideAssign(result.distance);
		}
		result.pointA.set(result.vectorAB);
		result.pointA.multiplyAssign(capsule.getRadius()).addAssign(closestPoint);
		result.distance -= capsule.getRadius();
	}

	// returns the distance between the two capsules
	public static void distanceCapsuleCapsule(Capsule capsuleA, Vector2f upVectorA, Vector2f positionA,
	                                          Capsule capsuleB, Vector2f upVectorB, Vector2f positionB,
	                                          DistanceResult result) {
		Vector2f pA, pB; // store the closest points on each capsule here
		final float PARALLEL_THRESHOLD = 0.9999f;
		float dot = MathOps.dot(upVectorA, upVectorB);
		float det = upVectorB.x()*upVectorA.y() - upVectorA.x()*upVectorB.y();
		if (Math.abs(dot) >= PARALLEL_THRESHOLD || det == 0.0f) {
			float halfHeightA = capsuleA.getHeight() * 0.5f;
			float halfHeightB = capsuleA.getHeight() * 0.5f;
			// lines are considered parallel
			// project B's points onto A's upVector
			// A's projection onto itself is -halfHeightA, halfHeightA
			Vector2f capsuleBVec = upVectorB.multiply(halfHeightB);
			Vector2f capsulePointB0 = positionB.subtract(capsuleBVec);
			Vector2f capsulePointB1 = positionB.add(capsuleBVec);
			float projB0 = MathOps.dot(capsulePointB0.subtract(positionA), upVectorA);
			float projB1 = MathOps.dot(capsulePointB1.subtract(positionA), upVectorA);
			if (projB1 < projB0) {
				float temp = projB0;
				projB0 = projB1;
				projB1 = temp;
			}
			// find intersection of projections by taking max of mins, min of maxes
			float isectMin = Math.max(-halfHeightA, projB0);
			float isectMax = Math.min(halfHeightA, projB1);
			// the intersection could be "negative" if they don't intersect
			// find the middle of the intersection and clamp to each individual projection's bounds
			float isectMid = (isectMin + isectMax) * 0.5f;
			float projA = MathOps.clamp(isectMid, -halfHeightA, halfHeightA);
			float projB = MathOps.clamp(isectMid, projB0, projB1);

			// we now have the projections of the two closest points
			// convert projB back in terms of upVectorB by projecting again
			Vector2f pointBOnA = upVectorA.multiply(projB).addAssign(positionA);
			float projBOnB = MathOps.dot(pointBOnA.subtract(positionB), upVectorB);
			// clamp just to be sure
			projBOnB = MathOps.clamp(projBOnB, -halfHeightB, halfHeightB);

			// finally, find the two points
			pA = upVectorA.multiply(projA).add(positionA);
			pB = upVectorB.multiply(projBOnB).add(positionB);
		} else {
			// solve for intersection of lines
			// oa + tda = ob + sdb
			// tda - sdb = ob - oa
			// t*dax - s*dbx = obx - oax
			// t*day - s*dby = oby - oay
			// [dax -dbx] [t]   [obx - oax]
			// [day -dby] [s] = [oby - oay]
			// invert matrix:
			// det = dbx*day - dax*dby
			float invDet = 1.0f / det;
			float inv00 = -upVectorB.y() * invDet;
			float inv01 = upVectorB.x() * invDet;
			float inv10 = -upVectorA.y() * invDet;
			float inv11 = upVectorA.x() * invDet;
			float vecA = positionA.x() - positionB.x();
			float vecB = positionA.y() - positionB.y();
			float t = inv00*vecA + inv01*vecB;
			float s = inv10*vecA + inv11*vecB;

			// determine if t and s are in range
			float halfHeightA = capsuleA.getHeight() * 0.5f;
			float halfHeightB = capsuleB.getHeight() * 0.5f;
			boolean tInRange = (t >= -halfHeightA && t <= halfHeightA);
			boolean sInRange = (s >= -halfHeightB && s <= halfHeightB);
			if (tInRange && sInRange) { // t, s
				// both points are same - the intersection point
				pA = pB = upVectorA.multiply(t).addAssign(positionA);
			} else if (tInRange) { // t, !s
				// t is in range but s is not - clamp s and recompute t
				s = MathOps.clamp(s, -halfHeightB, halfHeightB);
				pB = upVectorB.multiply(s).addAssign(positionB);
				t = MathOps.dot(pB.subtract(positionA), upVectorA);
				t = MathOps.clamp(t, -halfHeightA, halfHeightA);
				pA = upVectorA.multiply(t).addAssign(positionA);
			} else if (sInRange) { // !t, s
				// s is in range but t is not - clamp t and recompute s
				t = MathOps.clamp(t, -halfHeightA, halfHeightA);
				pA = upVectorA.multiply(t).addAssign(positionA);
				s = MathOps.dot(pA.subtract(positionB), upVectorB);
				s = MathOps.clamp(s, -halfHeightB, halfHeightB);
				pB = upVectorB.multiply(s).addAssign(positionB);
			} else { // !t, !s
				// neither is in range - clamp
				t = MathOps.clamp(t, -halfHeightA, halfHeightA);
				s = MathOps.clamp(s, -halfHeightB, halfHeightB);
				pA = upVectorA.multiply(t).addAssign(positionA);
				pB = upVectorB.multiply(s).addAssign(positionB);
			}
		}

		result.vectorAB.set(pB);
		result.vectorAB.subtractAssign(pA);
		result.distance = result.vectorAB.magnitude();
		float invDist = (result.distance == 0.0f) ? 0.0f : 1.0f / result.distance;
		result.vectorAB.multiplyAssign(invDist);
		result.pointA.set(result.vectorAB);
		result.pointA.multiplyAssign(capsuleA.getRadius());
		result.pointA.addAssign(pA);
		result.distance -= capsuleA.getRadius();
		result.distance -= capsuleB.getRadius();
	}

	// returns whether the capsule and AABB intersect
	public static boolean intersectCapsuleAabb(Capsule capsule, Vector2f upVector, Vector2f position,
	                                           Vector2f aabbMin, Vector2f aabbMax) {
		// given any point p, we can find its distance to the AABB
		// we want to find the closest point on the capsule's line segment to the AABB
		// this point will either be an endpoint or somewhere along the line
		// if it is a point along the line, we can find it by determining which corner is most "facing" the line

		float halfHeight = capsule.getHeight() * 0.5f;

		// first find the distance between AABB and capsule line
		// find which corner to check based on the direction of the line
		// i.e. pick the corner which the normal "points" most toward
		Vector2f cA, cB;
		if (Math.signum(upVector.x()) == Math.signum(upVector.y())) {
			// check corners -x,+y and +x,-y
			cA = new Vector2f(aabbMin.x(), aabbMax.y());
			cB = new Vector2f(aabbMax.x(), aabbMin.y());
		} else {
			// check corners -x,-y and +x,+y
			cA = new Vector2f(aabbMin.x(), aabbMin.y());
			cB = new Vector2f(aabbMax.x(), aabbMax.y());
		}
		// determine the closest points to each of the corners on the line (clamped)
		float projA = MathOps.dot(cA.subtract(position), upVector);
		projA = MathOps.clamp(projA, -halfHeight, halfHeight);
		Vector2f lpA = upVector.multiply(projA).addAssign(position);

		float projB = MathOps.dot(cB.subtract(position), upVector);
		projB = MathOps.clamp(projB, -halfHeight, halfHeight);
		Vector2f lpB = upVector.multiply(projB).addAssign(position);

		// find distance between AABB and endpoints
		Vector2f capsuleVec = upVector.multiply(halfHeight);
		Vector2f pA = position.subtract(capsuleVec);
		Vector2f pB = position.add(capsuleVec);

		// clamp points to AABB bounds
		Vector2f closestPA = MathOps.clamp(pA, aabbMin, aabbMax);
		Vector2f closestPB = MathOps.clamp(pB, aabbMin, aabbMax);
		Vector2f closestLPA = MathOps.clamp(lpA, aabbMin, aabbMax);
		Vector2f closestLPB = MathOps.clamp(lpB, aabbMin, aabbMax);
		float dist2 = Math.min(
				Math.min(MathOps.distanceSquared(pA, closestPA), MathOps.distanceSquared(pB, closestPB)),
				Math.min(MathOps.distanceSquared(lpA, closestLPA), MathOps.distanceSquared(lpB, closestLPB)));
		return dist2 <= capsule.getRadius()*capsule.getRadius();
	}

	// returns whether the circle and AABB intersect
	public static boolean intersectCircleAabb(float radius, Vector2f position,
	                                          Vector2f aabbMin, Vector2f aabbMax) {
		// clamp points to AABB bounds
		Vector2f closestP = MathOps.clamp(position, aabbMin, aabbMax);
		float dist2 = MathOps.distanceSquared(position, closestP);
		return dist2 <= radius*radius;
	}

	// returns whether the capsule and circle intersect
	public static boolean intersectCapsuleCircle(Capsule capsule, Vector2f upVector, Vector2f position,
	                                             Vector2f center, float radius) {
		// find closest point on capsule's line
		float proj = MathOps.dot(center.subtract(position), upVector);
		// clamp to [-height/2, height/2]
		float halfHeight = capsule.getHeight() * 0.5f;
		proj = MathOps.clamp(proj, -halfHeight, halfHeight);
		Vector2f closest = upVector.multiply(proj).addAssign(position);
		float radSum = radius + capsule.getRadius();
		return (MathOps.distanceSquared(closest, center) <= radSum*radSum);
	}

	public static void sweepPointAgainstSegment(Ray2f ray,
	                                            Vector2f[] vertices, Vector2f normal,
	                                            SweepResult result) {
		result.t = Float.POSITIVE_INFINITY;

		float nDotD = MathOps.dot(normal, ray.getDirection());
		if (nDotD >= 0.0f) {
			// no intersection
			return;
		}

		Vector2f ao = ray.getOrigin().subtractAssign(vertices[0]);
		float t = MathOps.dot(normal, ao) / -nDotD;

		// determine if point is within range
		Vector2f p = ray.getDirection().multiplyAssign(t).addAssign(ao);
		Vector2f lineVec = vertices[1].subtract(vertices[0]);
		float proj = MathOps.dot(p, lineVec);
		if (proj >= 0.0f && proj <= lineVec.magnitudeSquared()) {
			result.t = t;
			result.point.set(p.addAssign(vertices[0]));
			result.normal.set(normal);
		}
	}

	// casts a ray against the capsule and returns the time, point, and normal of intersection
	// if the ray's origin intersects the capsule, the resulting normal is zero
	public static void sweepPointAgainstCapsule(Ray2f ray,
	                                            Capsule capsule, Vector2f upVector, Vector2f position,
	                                            SweepResult result) {
		if (capsule.getHeight() == 0.0f) {
			sweepPointAgainstCircle(ray, capsule.getRadius(), position, result);
			return;
		}

		result.t = Float.POSITIVE_INFINITY;

		// first determine if ray is already inside capsule by taking distance from origin to capsule's line segment
		float proj = MathOps.dot(ray.getOrigin().subtractAssign(position), upVector);
		float halfHeight = capsule.getHeight() * 0.5f;
		proj = MathOps.clamp(proj, -halfHeight, halfHeight);
		Vector2f closestPoint = upVector.multiply(proj).addAssign(position);
		if (MathOps.distanceSquared(closestPoint, ray.getOrigin()) <= capsule.getRadius()* capsule.getRadius()) {
			result.t = 0.0f;
			result.point.set(ray.getOrigin());
			result.normal.setZero();
		}

		// we first solve for intersection with line segment
		// then check endpoints

		// ax + by = c
		Vector2f normal = upVector.rotateCw();
		float c = MathOps.dot(normal, position);

		// d = n.p - c
		// p = o + td
		// d = n.(o + td) - c
		//   = n.o + tn.d - c
		// solve for when d = +-r
		// t = (+-r + c - n.o) / n.d
		float nDotD = MathOps.dot(normal, ray.getDirection());
		float nDotO = MathOps.dot(normal, ray.getOrigin());
		float t0 = (c - nDotO - capsule.getRadius()) / nDotD;
		float t1 = (c - nDotO + capsule.getRadius()) / nDotD;
		// lower t is when ray enters, greater t is when ray leaves
		// we only care about when ray enters
		float t = Math.min(t0, t1);
		if (t >= 0.0f) {
			// check whether the point is within the capsule's line
			Vector2f contactPoint = ray.getDirection().multiplyAssign(t).addAssign(ray.getOrigin()).subtractAssign(position);
			float contactProj = MathOps.dot(contactPoint, upVector);
			if (Math.abs(contactProj) <= halfHeight) {
				result.t = t;
				result.point.set(contactPoint);
				result.normal.set(normal);
				Vector2f positionToContact = contactPoint.subtract(position);
				float dot = MathOps.dot(result.normal, positionToContact);
				// if the contact point is on the same side as the normal vector points, use the normal directly
				// otherwise, negate it
				if (dot < 0.0f) {
					result.normal.negateAssign();
				}
				return;
			}
		}

		// check each endpoint
		Vector2f halfHeightVec = upVector.multiply(halfHeight);
		Vector2f negEndpoint = position.subtract(halfHeightVec);
		Vector2f posEndpoint = position.add(halfHeightVec);
		SweepResult res0 = new SweepResult();
		res0.point = new Vector2f();
		res0.normal = new Vector2f();
		SweepResult res1 = new SweepResult();
		res1.point = new Vector2f();
		res1.normal = new Vector2f();
		sweepPointAgainstCircle(ray, capsule.getRadius(), negEndpoint, res0);
		sweepPointAgainstCircle(ray, capsule.getRadius(), posEndpoint, res1);
		if (!Float.isInfinite(res0.t) || Float.isInfinite(res1.t)) {
			SweepResult finalResult = (res0.t < res1.t) ? res0 : res1;
			result.t = finalResult.t;
			result.point.set(finalResult.point);
			result.normal.set(finalResult.normal);
		}
	}

	// casts a ray against the circle and returns the time, point, and normal of intersection
	// if the ray's origin intersects the circle, the resulting normal is zero
	public static void sweepPointAgainstCircle(Ray2f ray,
	                                                float radius, Vector2f position,
	                                                SweepResult result) {
		result.t = Float.POSITIVE_INFINITY;

		float rad2 = radius*radius;

		// bring circle to origin
		Vector2f rayOrig = ray.getOrigin();
		rayOrig.subtractAssign(position);

		// check if we're starting from within the circle
		if (rayOrig.magnitudeSquared() <= rad2) {
			result.t = 0.0f;
			result.point.set(ray.getOrigin());
			result.normal.setZero();
			return;
		}

		// ray equation is p = o + td
		// want to solve for t such that p.p = r^2
		// r^2 = o.o + 2to.d + t^2d.d
		// t^2(d.d) + t(2o.d) + (o.o - r^2) = 0
		// solve using quadratic formula
		Vector2f rayDir = ray.getDirection();
		float a = rayDir.magnitudeSquared();
		float b = 2.0f * MathOps.dot(rayOrig, rayDir);
		float c = rayOrig.magnitudeSquared() - rad2;
		if (a != 0.0f) {
			float disc = b*b - 4.0f*a*c;
			if (disc >= 0.0f) {
				disc = (float) Math.sqrt(disc);
				float invDenom = 0.5f / a;
				float t0 = (-b - disc) * invDenom;
				float t1 = (-b + disc) * invDenom;
				// we only care about the time of entrance: whichever t is smaller
				float t = Math.min(t0, t1);
				if (t >= 0.0f) {
					result.t = t;
					result.point.set(ray.getDirection().multiplyAssign(result.t).addAssign(ray.getOrigin()));
					result.normal.set(position);
					result.normal.negateAssign().addAssign(result.point).normalizeAssign();
				}
			}
		}
	}

	// casts a ray/radius against the circle and returns the time, point, and normal of intersection
	// if the ray's origin intersects the circle, the resulting normal is zero
	public static void sweepCircleAgainstCircle(Ray2f ray, float rayRadius,
	                                           float radius, Vector2f position,
	                                           SweepResult result) {
		if (radius == 0.0f) {
			sweepPointAgainstCircle(ray, radius, position, result);
			return;
		}

		result.t = Float.POSITIVE_INFINITY;

		float radSum = rayRadius + radius;
		float radSum2 = radSum*radSum;

		// bring circle being cast against to origin
		Vector2f rayOrig = ray.getOrigin();
		rayOrig.subtractAssign(position);

		// check if we're starting from within the circle
		if (rayOrig.magnitudeSquared() <= radSum2) {
			result.t = 0.0f;
			result.point.set(ray.getOrigin());
			result.normal.setZero();
			return;
		}

		// let r be sum of radii
		// ray equation is p = o + td
		// want to solve for t such that p.p = r^2
		// r^2 = o.o + 2to.d + t^2d.d
		// t^2(d.d) + t(2o.d) + (o.o - r^2) = 0
		// solve using quadratic formula
		Vector2f rayDir = ray.getDirection();
		float a = rayDir.magnitudeSquared();
		float b = 2.0f * MathOps.dot(rayOrig, rayDir);
		float c = rayOrig.magnitudeSquared() - radSum2;
		if (a != 0.0f) {
			float disc = b*b - 4.0f*a*c;
			if (disc >= 0.0f) {
				disc = (float) Math.sqrt(disc);
				float invDenom = 0.5f / a;
				float t0 = (-b - disc) * invDenom;
				float t1 = (-b + disc) * invDenom;
				// we only care about the time of entrance: whichever t is smaller
				float t = Math.min(t0, t1);
				if (t >= 0.0f) {
					result.t = t;
					result.point.set(ray.getDirection().multiplyAssign(result.t).addAssign(ray.getOrigin()));
					result.normal.set(position);
					result.normal.negateAssign().addAssign(result.point).normalizeAssign();
					result.point.subtractAssign(result.normal.multiply(rayRadius));
				}
			}
		}
	}

	// casts a ray/radius against the capsule and returns the time, point, and normal of intersection
	// if the ray's origin intersects the capsule, the resulting normal is zero
	public static void sweepCircleAgainstCapsule(Ray2f ray, float rayRadius,
	                                             Capsule capsule, Vector2f upVector, Vector2f position,
	                                             SweepResult result) {
		Capsule c = new Capsule(capsule.getHeight(), capsule.getRadius() + rayRadius);
		sweepPointAgainstCapsule(ray, c, upVector, position, result);
		if (!result.normal.isZero()) {
			// if the normal isn't zero, adjust the point of intersection by moving it "back" by the ray's radius
			// (if the normal is zero we can't do much since the intersection was immediate)
			result.point.subtractAssign(result.normal.multiply(rayRadius));
		}
	}

	// casts a ray against the AABB and returns the time, point, and normal of intersection
	// if the ray's origin intersects the AABB, the resulting normal is zero
	public static void sweepPointAgainstAabb(Ray2f ray,
	                                         Vector2f aabbMin, Vector2f aabbMax,
	                                         SweepResult result) {
		result.t = Float.POSITIVE_INFINITY;

		Vector2f origin = ray.getOrigin();
		Vector2f direction = ray.getDirection();

		if (origin.x() >= aabbMin.x() && origin.y() >= aabbMin.y() &&
				origin.x() <= aabbMax.x() && origin.y() <= aabbMax.y()) {
			// already inside AABB - return instantly
			result.t = 0.0f;
			result.point.set(origin);
			result.normal.setZero();
			return;
		}

		Vector2f[] bounds = new Vector2f[] { aabbMin, aabbMax };
		for (int i = 0; i < 2; ++i) {
			// test intersection in each component direction (x and y)
			float origA = origin.get(i);
			float origB = origin.get(1-i);
			float dirA = direction.get(i);
			float dirB = direction.get(1-i);
			if (dirA == 0.0f) {
				continue;
			}
			for (int b = 0; b < 2; ++b) {
				// test each bound
				float bound = bounds[b].get(i);
				float t = (bound - origA) / dirA;
				// make sure value is in range on the other axis
				float pB = origB + t*dirB;
				if (pB >= aabbMin.get(1-i) && pB <= aabbMax.get(1-i)) {
					if (t < result.t) {
						result.t = t;
						result.point.set(direction.multiply(result.t).addAssign(origin));
						// set normal to either -1 or 1 for component we are testing, depending on bound being tested
						// set other component to 0
						result.normal.set(i, (b == 0) ? -1.0f : 1.0f);
						result.normal.set(1-i, 0.0f);
					}
				}
			}
		}
	}

	// casts a ray/radius against the AABB and returns the time, point, and normal of intersection
	// if the ray's origin intersects the AABB, the resulting normal is zero
	public static void sweepCircleAgainstAabb(Ray2f ray, float rayRadius,
	                                          Vector2f aabbMin, Vector2f aabbMax,
	                                          SweepResult result) {
		if (rayRadius == 0.0f) {
			sweepPointAgainstAabb(ray, aabbMin, aabbMax, result);
			return;
		}

		result.t = Float.POSITIVE_INFINITY;

		Vector2f origin = ray.getOrigin();
		Vector2f direction = ray.getDirection();

		if (intersectCircleAabb(rayRadius, origin, aabbMin, aabbMax)) {
			// immediate intersection
			result.t = 0.0f;
			result.point.set(origin);
			result.normal.setZero();
			return;
		}

		// first "expand" AABB to check collision against sides
		// however, once a collision is detected against the side, don't expand the range check for that edge
		// this cuts off the corners
		Vector2f[] bounds = new Vector2f[] { aabbMin, aabbMax };
		for (int i = 0; i < 2; ++i) {
			// test intersection in each component direction (x and y)
			float origA = origin.get(i);
			float origB = origin.get(1-i);
			float dirA = direction.get(i);
			float dirB = direction.get(1-i);
			if (dirA == 0.0f) {
				continue;
			}
			for (int b = 0; b < 2; ++b) {
				// test each bound - expand by +- radius, depending on if this is the min or max bound
				float bound = bounds[b].get(i) + ((b == 0) ? -rayRadius : rayRadius);
				float t = (bound - origA) / dirA;
				// make sure value is in range on the other axis - don't expand these bounds
				float pB = origB + t*dirB;
				if (pB >= aabbMin.get(1-i) && pB <= aabbMax.get(1-i)) {
					if (t < result.t) {
						result.t = t;
						result.point.set(i, origA + t*dirA);
						result.point.set(1-i, pB);
						// set normal to either -1 or 1 for component we are testing, depending on bound being tested
						// set other component to 0
						result.normal.set(i, (b == 0) ? -1.0f : 1.0f);
						result.normal.set(1-i, 0.0f);
						result.point.subtractAssign(result.normal.multiply(rayRadius));
					}
				}
			}
		}

		if (Float.isInfinite(result.t)) {
			return;
		}

		// now test each corner by casting a ray starting at the corner in the opposite direction
		SweepResult cornerResult = new SweepResult();
		cornerResult.point = new Vector2f();
		cornerResult.normal = new Vector2f();
		direction.negateAssign();
		for (int y = 0; y < 2; ++y) {
			for (int x = 0; x < 2; ++x) {
				Ray2f cornerRay = new Ray2f(
						new Vector2f(bounds[x].x(), bounds[y].y()), direction);
				sweepPointAgainstCircle(cornerRay, rayRadius, origin, cornerResult);
				if (cornerResult.t < result.t) {
					result.t = cornerResult.t;
					result.point.set(bounds[x].x(), bounds[y].y());
					result.normal.set(cornerResult.normal);
					result.normal.negateAssign();
				}
			}
		}
	}
}
